Enabled sending waypoints to ardupilot for copter and rover #26362

haarshitgarg · 2024-02-29T17:25:03Z

Way point support for Copter

This is to fix the issue #25381

Implemented set_global_location for Rover and Copter ( taking inspiration from ArduPlane)

Demo

Taking everything from this pull request

In ArduPilot repo:

./sim_vehicle.py -v ArduCopter --map --console --enable-dds

Running MicroROS agent:

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_sitl
. install/setup.bash
cd src/ardupilot/libraries/AP_DDS
ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml

Taking off the copter

mode guided
arm throttle
takeoff 50

Now publish a message for drone to go to greyhound track.

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_msgs
. install/setup.bash
ros2 topic pub /ap/cmd_gps_pose  ardupilot_msgs/msg/GlobalPosition "{header: {frame_id: map}, latitude: -35.345996, longitude: 149.159017, altitude: 40, coordinate_frame: 6}" --once

Way point support for copter Rover

Follow all the steps as for copter above

Demo Rover

Same as copter (plus altitude is ignored in ros message)
Result Screenshot (Rover going to the same location as copter)

amilcarlucas · 2024-03-02T11:13:24Z

Can most of this be moved to AP_Vehicle instead?

haarshitgarg · 2024-03-03T14:27:32Z

Can most of this be moved to AP_Vehicle instead?

Hello @amilcarlucas I am not really sure about that. I am just following the structure already being followed for ArduPlane.

Ryanf55 · 2024-03-03T20:03:21Z

Can most of this be moved to AP_Vehicle instead?

Which parts do you think should be moved? Collapse AP_ExternalControl in AP_Vehicle, or something else?

amilcarlucas · 2024-03-04T13:22:32Z

Anything that is common and can be reuse is a good candidate.

That way sub and blimp will also get (at least parts) of this feature.

Ryanf55

Overall, this is looking great. Sorry for the delay on the review. I forgot to hit submit.
I'll run testing on it shortly. Have you tried using the python script I wrote to send a goal, and try it against copter?

ArduCopter/Copter.cpp

ArduCopter/AP_ExternalControl_Copter.cpp

ArduCopter/Copter.cpp

rmackay9 · 2024-03-14T00:04:03Z

This is looking pretty good I think, it would be good to squash some of the commits together and add the regular subsystem prefixes to them. E.g. commits affecting rover should start with "Rover: ", commits affecting Copter should start with "Copter: ". Also we should have any merge commits. Thanks!

Ryanf55 · 2024-04-16T00:42:25Z

This is looking pretty good I think, it would be good to squash some of the commits together and add the regular subsystem prefixes to them. E.g. commits affecting rover should start with "Rover: ", commits affecting Copter should start with "Copter: ". Also we should have any merge commits. Thanks!

Hi @haarshitgarg , Do you mind fixing up the commits? This is looking nice and I'd like to get this in. We had another asking for copter waypoints on the discord thread.

maximecapelle · 2024-04-30T11:00:52Z

Is there a way to takeoff throught the ros2 topics aswell? I have used the additions you suggested and the waypoint navigation works when it is in the air but not to takeoff. It would be nice if we can set the drone to mode guided, arm and takeoff all through the ros2 topics! :)

rmackay9 · 2024-04-30T11:09:03Z

@maximecapelle,

I'm not really familiar with this area but maybe this PR is what you're looking for? #26911

maximecapelle · 2024-04-30T12:15:00Z

@maximecapelle,

I'm not really familiar with this area but maybe this PR is what you're looking for? #26911

Thank you! I’ll check it out :)

maximecapelle · 2024-05-01T11:03:42Z

Also, is there a way to use waypoints in the local coordinate frame? (x, y, z) Using the "/ap/pose/filtered" topic msgs you have your position in the local map coordinates. Could someone help me implement that? Sorry I have little knowledge on Ardupilot source code haha.

Ryanf55 · 2024-05-01T13:46:28Z

Also, is there a way to use waypoints in the local coordinate frame? (x, y, z) Using the "/ap/pose/filtered" topic msgs you have your position in the local map coordinates. Could someone help me implement that? Sorry I have little knowledge on Ardupilot source code haha.

No, that's not part of REP-147. ArduPilot's waypoints are in WGS-84 (global) coordinates. If you want to do planning on local coordinates that's fine, but transform to global before sending to the autopilot.

Ryanf55

Aside from the style changes, the functionality is spot-on. Well done!

I just force pushed the style fixes, enforced it in run_astyle.py and finally fixed the commit messages.

…dupilot for copter and rover Signed-off-by: Ryan Friedman <[email protected]>

…ot for copter and rover Signed-off-by: Ryan Friedman <[email protected]>

Ryanf55 · 2024-05-01T21:40:04Z

@haarshitgarg Would you be interested in a doing a follow-up PR allowing users to set a yaw angle at the waypoint? This would be good for inspection use cases with a fixed camera where you point the drone at a cliff/wall at each waypoint.

rmackay9

Sorry to be nitpicky about naming but could we change this to clarify that we're setting a target location? we normally use the word "position" to indicate an offset from the EKF origin and "location" for lat, lon, alt. Not specifying that this is a target (e.g. controller target) rather than an [ekf] estimate is important as well I think.

Ryanf55 · 2024-05-02T01:01:26Z

Sorry to be nitpicky about naming but could we change this to clarify that we're setting a target location? we normally use the word "position" to indicate an offset from the EKF origin and "location" for lat, lon, alt. Not specifying that this is a target (e.g. controller target) rather than an [ekf] estimate is important as well I think.

Do you want the functions renamed in Plane and Copter, or just External control?

rmackay9 · 2024-05-02T01:12:23Z

Hi @Ryanf55,

I think set_global_position() should be renamed to set_target_location(). That will lead to identically named methods in the Copter and Rover vehicle class and in AP_ExternalControl but I think that's OK... maybe other devs will disagree but we can clear that up on the dev call if not before.

Ryanf55 · 2024-05-02T02:58:41Z

Ok. It's currently named based on REP-147: https://ros.org/reps/rep-0147.html#goal-interface

haarshitgarg marked this pull request as draft March 3, 2024 15:24

haarshitgarg marked this pull request as ready for review March 3, 2024 16:36

haarshitgarg mentioned this pull request Mar 3, 2024

AP_DDS: Add waypoint support #25381

Closed

Ryanf55 self-requested a review March 3, 2024 19:57

Ryanf55 added the ROS label Mar 3, 2024

Ryanf55 reviewed Mar 13, 2024

View reviewed changes

ArduCopter/Copter.cpp Show resolved Hide resolved

rmackay9 reviewed Mar 13, 2024

View reviewed changes

ArduCopter/AP_ExternalControl_Copter.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Mar 14, 2024

View reviewed changes

ArduCopter/Copter.cpp Show resolved Hide resolved

Ryanf55 self-assigned this May 1, 2024

Ryanf55 self-requested a review May 1, 2024 17:56

Ryanf55 added this to the DDS 4.6 milestone May 1, 2024

Ryanf55 approved these changes May 1, 2024

View reviewed changes

haarshitgarg added 3 commits May 1, 2024 13:43

ArduCopter: enabled sending waypoints from a companion computer to ar…

06138fd

…dupilot for copter and rover Signed-off-by: Ryan Friedman <[email protected]>

Rover: enabled sending waypoints from a companion computer to ardupil…

53f4867

…ot for copter and rover Signed-off-by: Ryan Friedman <[email protected]>

Tools: enabled sending waypoints from a companion computer to ardupil…

f66e85d

…ot for copter and rover Signed-off-by: Ryan Friedman <[email protected]>

Ryanf55 force-pushed the pr/issue25381 branch from 8ee7ab3 to f66e85d Compare May 1, 2024 19:44

Ryanf55 added the MergeOnCIPass label May 1, 2024

rmackay9 reviewed May 2, 2024

View reviewed changes

rmackay9 merged commit 4c95a3b into ArduPilot:master May 2, 2024
76 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Enabled sending waypoints to ardupilot for copter and rover #26362

Enabled sending waypoints to ardupilot for copter and rover #26362

haarshitgarg commented Feb 29, 2024 •

edited

Loading

amilcarlucas commented Mar 2, 2024

haarshitgarg commented Mar 3, 2024

Ryanf55 commented Mar 3, 2024

amilcarlucas commented Mar 4, 2024

Ryanf55 left a comment

rmackay9 commented Mar 14, 2024 •

edited

Loading

Ryanf55 commented Apr 16, 2024

maximecapelle commented Apr 30, 2024

rmackay9 commented Apr 30, 2024

maximecapelle commented Apr 30, 2024

maximecapelle commented May 1, 2024

Ryanf55 commented May 1, 2024 •

edited

Loading

Ryanf55 left a comment •

edited

Loading

Ryanf55 commented May 1, 2024 •

edited

Loading

rmackay9 left a comment

Ryanf55 commented May 2, 2024

rmackay9 commented May 2, 2024

Ryanf55 commented May 2, 2024

Enabled sending waypoints to ardupilot for copter and rover #26362

Enabled sending waypoints to ardupilot for copter and rover #26362

Conversation

haarshitgarg commented Feb 29, 2024 • edited Loading

Way point support for Copter

Demo

Way point support for copter Rover

Demo Rover

amilcarlucas commented Mar 2, 2024

haarshitgarg commented Mar 3, 2024

Ryanf55 commented Mar 3, 2024

amilcarlucas commented Mar 4, 2024

Ryanf55 left a comment

Choose a reason for hiding this comment

rmackay9 commented Mar 14, 2024 • edited Loading

Ryanf55 commented Apr 16, 2024

maximecapelle commented Apr 30, 2024

rmackay9 commented Apr 30, 2024

maximecapelle commented Apr 30, 2024

maximecapelle commented May 1, 2024

Ryanf55 commented May 1, 2024 • edited Loading

Ryanf55 left a comment • edited Loading

Choose a reason for hiding this comment

Ryanf55 commented May 1, 2024 • edited Loading

rmackay9 left a comment

Choose a reason for hiding this comment

Ryanf55 commented May 2, 2024

rmackay9 commented May 2, 2024

Ryanf55 commented May 2, 2024

haarshitgarg commented Feb 29, 2024 •

edited

Loading

rmackay9 commented Mar 14, 2024 •

edited

Loading

Ryanf55 commented May 1, 2024 •

edited

Loading

Ryanf55 left a comment •

edited

Loading

Ryanf55 commented May 1, 2024 •

edited

Loading